#pragma once
#include "..\structure\Enriched_polyhedron.h"

using std::pair;
using std::string;
class GLFont;

#define NumJoints	18
#define NumBones	17
#define NumPartCut	25

template <class Polyhedron, class Kernel>
class CSkeleton
{
public:
	typedef typename Kernel::FT			FT;
	typedef typename Kernel::Point_3	Point_3;
	typedef typename Kernel::Vector_3	Vector_3;
	typedef typename Polyhedron::Normal Normal; 
	typedef typename Polyhedron::Vertex_const_iterator		Vertex_const_iterator;
	typedef typename Polyhedron::Facet_iterator				Facet_iterator;
	typedef typename Polyhedron::Facet_const_iterator		Facet_const_iterator;
	typedef typename Polyhedron::Halfedge_around_facet_circulator			HAF_circulator;
	typedef typename Polyhedron::Halfedge_around_facet_const_circulator		HAF_const_circulator;
	typedef typename Polyhedron::Halfedge_around_vertex_const_circulator	HAV_const_circulator;
	typedef typename Polyhedron::Halfedge_around_vertex_circulator			HAV_circulator;
	typedef std::pair<Vector3,int>		NodeType;
	typedef std::pair<int,int>			BoneType;
	typedef std::map<int, NodeType>		MAP_IND;	// map int to point3
	typedef std::multimap<int, int>		TREE_II;	// map int to point3
public:
	Polyhedron	*m_pMesh;
	MAP_IND		m_NodeMap;
	CString		m_fileName;
	std::vector<BoneType>	m_Bone2JointIdxs;   
	TREE_II m_JointTree;	/*Hierarchy: first is bone_root, is key, is the 2nd number of every row in BoneLinkage.txt...*/
	static	int	m_LabelOrder2TreeOrder_Bone[NumBones];   
	unsigned	m_nMeshV;
	std::vector<Vector3> m_JointPos,	m_joint_copy;  //in Joint_Label order
	bool m_bJointsCopied;
	std::vector<Vector3> m_BoneVct;   //in JointTree order
	std::vector<FT> m_BoneLen;		  //in JointTree order 		
	std::vector<Vector3> m_JointsTagPos,	m_BonesTagPos;  //in Joint_Label order
	GLuint		m_skeleton_display_list;
	FT			m_sf;	
	Vector3		m_center;
	//for LBS deformation	/*all in Joint_Label order for fast recursive*/
	std::vector<FT *>		m_BoneWeights;   
	std::vector<FT>			m_BoneScale; 
	std::vector<Transform<> >	m_BoneTransform; 
	//for bodypart segment by boneweight
	std::vector<int>	V_part_label; //map Vidx to part idx
	std::vector<int>	F_part_label; //map Fidx to part idx	
	std::vector<int>	F_part_colorTag;
	bool m_bColorTagged;
	//pick and adjust:
	std::set<int> picked_joints,picked_bones;
	std::vector<std::set<int> >  m_JointNbrBone;	//decide for each joints which body parts are connected by it.
	/* CAUTION: Joint_Label order is here, according to the file reading process, has nothing to do with the tree order,you know...*/
	enum	JOINT_LABEL{chest=0,navel,waist,head,   
						pelvisR,kneeR,ankleR,footR,
						pelvisL,kneeL,ankleL,footL,
						shoulderR,elbowR,handR,
						shoulderL,elbowL,handL};
	static	string		m_strJointLabel[NumJoints];
	enum	BONE_LABEL{	B_sternum=0,B_lumbar,B_neck,
						B_huckleboneR,B_femurR,B_tibiaR,B_metatarsalR,
 						B_huckleboneL,B_femurL,B_tibiaL,B_metatarsalL,
 						B_clavicleR,B_humerusR,B_radiusR,
				 		B_clavicleL,B_humerusL,B_radiusL};
 	static string		m_strBoneLabel[NumBones];
	enum	Cut_Label { C_headTop=0, C_neck_sternum,C_neck_clavicleL,C_neck_clavicleR,
						C_clavicle_humerusL, C_humerus_RadiusL, C_wristL,
						C_clavicle_humerusR, C_humerus_RadiusR, C_wristR,
						C_clavicle_sternumL, C_clavicle_sternumR, C_sternum_humerusL, C_sternum_humerusR,	//upper ring of the body,5 of them.
						C_sternum_lumbar,																	//lower ring of the body,just 1. 
						C_lumbar_huckleboneL,C_lumbar_huckleboneR,
						C_hucklebone_femurL, C_hucklebone_femurR,
						C_femur_tibiaL,		 C_femur_tibiaR,
						C_tibia_metatarsalL, C_tibia_metatarsalR,
						C_toeL, C_toeR};
	GLFont	*pGLFont;
	
public:
	CSkeleton();

	CSkeleton(Polyhedron *pMesh);
	
	~CSkeleton();

	bool LoadSkeletonFromFile(std::ifstream &stream, double given_scalef=0.0, bool bUnitToMeshCenter=true,bool ReCalcBoneData = true);

	void LoadJointsFromFile(std::ifstream &stream);

	bool SaveSkeletonIntoFile(std::ofstream &stream);
	
	void SetSkeletonName(CString path){	m_fileName = path;}

	CString GetSkeletonName(){	return m_fileName;}

	void JointTagPosition();

	void unitize_skeleton(double given_scalef=0.0, bool bUnitToMeshCenter = true, bool ReCalcBoneData = true);
	
	void BuildSkeletonDisplayList(bool smooth_shading=true);

	void gl_draw_skeleton();
	
	void gl_draw_joints_tag(CSize Wnd_Size, bool bOrthoProj=true);
	
	void gl_draw_bones_tag(CSize Wnd_Size, bool bOrthoProj=true);
	
	void LoadBoneWeights(std::ifstream &stream);

	void LBSDeformation(std::vector<Vector3> &NewJointPos);

	void BoneTransform (std::vector<Vector3> &NewJointPos);
	
	bool SegmentUsingBoneWeights();

	void bodypart_color_tag();

	void gl_draw_bodypart(CPseudoColorRGB &PsdC, bool smooth_shading=true);

	void clear_picked_joints(void){	picked_joints.clear();	}
	void clear_picked_bones(void){	picked_bones.clear();	}

	int pick_joint_by_ray(Vector3 &sp, Vector3 &d, float epsilon,bool bMuiltipick=false,bool bUnpick = false);
	int pick_bone_by_ray(Vector3 &sp, Vector3 &d, float epsilon,bool bMuiltipick=false,bool bUnpick = false);

	int pick_joints_by_column(Vector3 &sp, Vector3 &dx, Vector3 &dy, Vector3 &dz);
	int pick_bones_by_column(Vector3 &sp, Vector3 &dx, Vector3 &dy, Vector3 &dz);

	void unpick_joints_by_column(Vector3 &sp, Vector3 &dx, Vector3 &dy, Vector3 &dz);
	void unpick_bones_by_column(Vector3 &sp, Vector3 &dx, Vector3 &dy, Vector3 &dz);
		
	bool pick_joints_by_idx(int jidx);
	bool pick_bones_by_idx(int bidx);
		
	void gl_draw_picked_joints(void);
	void gl_draw_picked_bones(void);

	void Drag_Joints_By_Mouse(CPoint move, Matrix3 rot_matx,GLdouble *modelview, GLdouble *projection, GLint *viewport, bool bRigid=false);

	void Rotate(const Quaternion<> &rot);

	void Hold_Copy(bool bForce=false){
		if(!m_bJointsCopied || bForce ){
			m_joint_copy.clear(); m_joint_copy.resize(NumJoints);
			m_joint_copy.assign(m_JointPos.begin(),m_JointPos.end()); 
			m_bJointsCopied = true;
		}
	}

	void Copy_Back(){
		m_JointPos.assign(m_joint_copy.begin(),m_joint_copy.end()); 
	};
	
	void JointsConnectedBodyParts(){		
		m_JointNbrBone.clear(); m_JointNbrBone.resize(NumJoints);

		m_JointNbrBone[chest].insert(B_sternum);	m_JointNbrBone[chest].insert(B_neck); 
		m_JointNbrBone[chest].insert(B_clavicleL);	m_JointNbrBone[chest].insert(B_clavicleR);

		m_JointNbrBone[navel].insert(B_sternum);	m_JointNbrBone[navel].insert(B_lumbar); 

		m_JointNbrBone[waist].insert(B_lumbar);		m_JointNbrBone[waist].insert(B_huckleboneL);	m_JointNbrBone[waist].insert(B_huckleboneR); 

		m_JointNbrBone[head].insert(B_neck);

		m_JointNbrBone[pelvisR].insert(B_huckleboneR);	m_JointNbrBone[pelvisR].insert(B_femurR); 

		m_JointNbrBone[pelvisL].insert(B_huckleboneL);	m_JointNbrBone[pelvisL].insert(B_femurL); 

		m_JointNbrBone[kneeL].insert(B_femurL);			m_JointNbrBone[kneeL].insert(B_tibiaL); 
		
		m_JointNbrBone[kneeR].insert(B_femurR);			m_JointNbrBone[kneeR].insert(B_tibiaR); 

		m_JointNbrBone[ankleL].insert(B_tibiaL);		m_JointNbrBone[ankleL].insert(B_metatarsalL); 

		m_JointNbrBone[ankleR].insert(B_tibiaR);		m_JointNbrBone[ankleR].insert(B_metatarsalR); 

		m_JointNbrBone[footL].insert(B_metatarsalL);	m_JointNbrBone[footR].insert(B_metatarsalR);
		//upper body is problematic
		m_JointNbrBone[shoulderL].insert(B_clavicleL);	m_JointNbrBone[shoulderL].insert(B_humerusL);	m_JointNbrBone[shoulderL].insert(B_radiusL); 

		m_JointNbrBone[shoulderR].insert(B_clavicleR);	m_JointNbrBone[shoulderR].insert(B_humerusR);	m_JointNbrBone[shoulderR].insert(B_radiusR); 

		m_JointNbrBone[elbowL].insert(B_humerusL);		m_JointNbrBone[elbowL].insert(B_radiusL);	m_JointNbrBone[elbowL].insert(B_clavicleL);

		m_JointNbrBone[elbowR].insert(B_humerusR);		m_JointNbrBone[elbowR].insert(B_radiusR);	m_JointNbrBone[elbowR].insert(B_clavicleR);

		m_JointNbrBone[handL].insert(B_radiusL); 		m_JointNbrBone[handR].insert(B_radiusR); 
	}

	void LoadSegInfo(std::ifstream& stream){
		V_part_label.clear(); V_part_label.resize(m_nMeshV, B_radiusL);
		unsigned ptidx, npv, vidx;
		stream>>ptidx>>npv;
		if(ptidx!=B_sternum){
			MSG_BOX_ERROR("ptidx!=B_sternum");	return;
		}

		for (unsigned i=0; i<npv; ++i){
			stream>>vidx;
			V_part_label[vidx] = B_sternum;
		}

		unsigned n_Tri = m_pMesh->size_of_facets();
		F_part_label.clear(); F_part_label.resize(n_Tri);

		int Vpl[3];
		Facet_const_iterator Fit = m_pMesh->facets_begin();
		Facet_const_iterator Fbegin = m_pMesh->facets_end();
		int i = 0;
		CGAL_For_all(Fit,Fbegin){ 

			Vpl[0] = V_part_label[Fit->facet_begin()->vertex()->id()];
			Vpl[1] = V_part_label[Fit->facet_begin()->next()->vertex()->id()];
			if(Vpl[1]==Vpl[0])
				F_part_label[i] = Vpl[0];
			else	{
				Vpl[2] = V_part_label[Fit->facet_begin()->prev()->vertex()->id() ];
				if(Vpl[2]==Vpl[0]||Vpl[2]==Vpl[1])
					F_part_label[i] = Vpl[2];
				else{	//the 3 vertex part labels of %dth triangle are total different, need to find which V has the biggest bone weight;
					int maxid = MaxIdx(
						m_BoneWeights[Fit->facet_begin()->vertex()->id()][Vpl[0]],
						m_BoneWeights[Fit->facet_begin()->next()->vertex()->id()][Vpl[1]],
						m_BoneWeights[Fit->facet_begin()->prev()->vertex()->id()][Vpl[2]]
					);
					F_part_label[i] = Vpl[maxid];
				}
			}
			++i;
		}
		m_bColorTagged = false;
	}
	
	std::vector<std::vector<int> >  m_PartCut;  
	std::vector< std::vector< pair<int,int> > >m_CutLink;
	/*CAUTION:Cut on the boundary portion neighboring part-part,we call it ring.
	thus,for every bone, two end points, let the 1st ring is around the root.cool.*/
	bool CutPartCut(CSkeleton* pSkt=NULL)
	{
		if(V_part_label.size() != m_nMeshV){
			std::cout<<"body part segmentation is not ready, redo segmentation by bone weight now.\n"; 
			if(!SegmentUsingBoneWeights()) 
				return false;
		}
		
		m_PartCut.clear();	m_PartCut.resize(NumPartCut);

		Vertex_const_iterator Vit = m_pMesh->vertices_begin();
		Vertex_const_iterator Vend= m_pMesh->vertices_end();
		int vid(0);
		
		int vpl , nbrpl;
		HAV_const_circulator HE_Vit, HE_Vend;
		CGAL_For_all(Vit,Vend){ 			
			vid = Vit->id();
			vpl = V_part_label[vid];
			HE_Vit  = Vit->vertex_begin();
			HE_Vend = Vit->vertex_begin();			
			CGAL_For_all(HE_Vit, HE_Vend){
				int nbrVid = HE_Vit->opposite()->vertex()->id();
				nbrpl = V_part_label[ nbrVid ]; 
				if(V_part_label[ nbrVid ] != vpl ){
					if( vpl==B_neck )
					{ 
						if( nbrpl==B_sternum )
							m_PartCut[C_neck_sternum].push_back(vid);
						else if( nbrpl==B_clavicleL )
							m_PartCut[C_neck_clavicleL].push_back(vid);
						else if( nbrpl==B_clavicleR )
							m_PartCut[C_neck_clavicleR].push_back(vid);
					}
					else if( vpl==B_sternum){
						if(nbrpl==B_neck)
							m_PartCut[C_neck_sternum].push_back(vid);
						else if(nbrpl==B_clavicleL)
							m_PartCut[C_clavicle_sternumL].push_back(vid);
						else if(nbrpl==B_clavicleR)
							m_PartCut[C_clavicle_sternumR].push_back(vid);
						else if(nbrpl==B_humerusL)
							m_PartCut[C_sternum_humerusL].push_back(vid);
						else if(nbrpl==B_humerusR)
							m_PartCut[C_sternum_humerusR].push_back(vid);						
					}
					else if( vpl==B_clavicleL){
						if(nbrpl==B_neck)
							m_PartCut[C_neck_clavicleL].push_back(vid);
						else	if(nbrpl==B_sternum)
							m_PartCut[C_clavicle_sternumL].push_back(vid);
						else	if(nbrpl==B_humerusL)
							m_PartCut[C_clavicle_humerusL].push_back(vid);
					}
					else if( vpl==B_clavicleR){
						if(nbrpl==B_neck)
							m_PartCut[C_neck_clavicleR].push_back(vid);
						else	if(nbrpl==B_sternum)
							m_PartCut[C_clavicle_sternumR].push_back(vid);
						else	if(nbrpl==B_humerusR)
							m_PartCut[C_clavicle_humerusR].push_back(vid);
					}
					else if( vpl==B_humerusL){
						if(nbrpl==B_clavicleL)
							m_PartCut[C_clavicle_humerusL].push_back(vid);
						else	if(nbrpl==B_sternum)
							m_PartCut[C_sternum_humerusL].push_back(vid);
						else	if(nbrpl==B_radiusL)
							m_PartCut[C_humerus_RadiusL].push_back(vid);
					}
					else if( vpl==B_humerusR){
						if(nbrpl==B_clavicleR)
							m_PartCut[C_clavicle_humerusR].push_back(vid);
						else	if(nbrpl==B_sternum)
							m_PartCut[C_sternum_humerusR].push_back(vid);
						else	if(nbrpl==B_radiusR)
							m_PartCut[C_humerus_RadiusR].push_back(vid);
					}
					else if( vpl==B_radiusL){
						if(nbrpl==B_humerusL)
							m_PartCut[C_humerus_RadiusL].push_back(vid);						
					}
					else if( vpl==B_radiusR){
						if(nbrpl==B_humerusR)
							m_PartCut[C_humerus_RadiusR].push_back(vid);						
					}
					else if( vpl==B_lumbar){
						if(nbrpl==B_sternum)
							m_PartCut[C_sternum_lumbar].push_back(vid);						
						else	if(nbrpl==B_huckleboneL)
							m_PartCut[C_lumbar_huckleboneL].push_back(vid);						
						else	if(nbrpl==B_huckleboneR)
							m_PartCut[C_lumbar_huckleboneR].push_back(vid);						
					}
					else if( vpl==B_huckleboneL){
						if(nbrpl==B_femurL)
							m_PartCut[C_hucklebone_femurL].push_back(vid);						
						else	if(nbrpl==B_lumbar)
							m_PartCut[C_lumbar_huckleboneL].push_back(vid);												
					}
					else if( vpl==B_huckleboneR){
						if(nbrpl==B_femurR)
							m_PartCut[C_hucklebone_femurR].push_back(vid);						
						else	if(nbrpl==B_lumbar)
							m_PartCut[C_lumbar_huckleboneR].push_back(vid);												
					}
					else if( vpl==B_tibiaL){
						if(nbrpl==B_femurL)
							m_PartCut[C_femur_tibiaL].push_back(vid);						
						else	if(nbrpl==B_metatarsalL)
							m_PartCut[C_tibia_metatarsalL].push_back(vid);												
					}
					else if( vpl==B_tibiaR){
						if(nbrpl==B_femurR)
							m_PartCut[C_femur_tibiaR].push_back(vid);						
						else	if(nbrpl==B_metatarsalR)
							m_PartCut[C_tibia_metatarsalR].push_back(vid);												
					}					
				}
			}
		}
		//for headTop and 4 tipEnds load from picke_vertices files 
		std::ifstream stream;
		string buf; unsigned nEndTipV;
		//head Top
		CString path = G_RootFolder+"Template\\headTop.po";
		stream.open(path);
		if(!stream)
			MSG_BOX_ERROR("can't find/open file from %s", path.GetBuffer());
		else{
			stream >> buf >> nEndTipV;
			m_PartCut[C_headTop].clear();m_PartCut[C_headTop].resize(nEndTipV);
			for (unsigned i=0; i<nEndTipV; ++i)
				stream >> m_PartCut[C_headTop][i];			
		}
		stream.close();stream.clear();
		//left wrist
		path = G_RootFolder+"Template\\wristL.po";	stream.open(path);
		if(!stream)
			MSG_BOX_ERROR("can't find/open file from %s", path.GetBuffer());
		else{
			stream >> buf >> nEndTipV;
			m_PartCut[C_wristL].clear();m_PartCut[C_wristL].resize(nEndTipV);
			for (unsigned i=0; i<nEndTipV; ++i)
				stream >> m_PartCut[C_wristL][i];			
		}
		stream.close();stream.clear();
		//right wrist
		path = G_RootFolder+"Template\\wristR.po";	stream.open(path);
		if(!stream)
			MSG_BOX_ERROR("can't find/open file from %s", path.GetBuffer());
		else{
			stream >> buf >> nEndTipV;
			m_PartCut[C_wristR].clear();m_PartCut[C_wristR].resize(nEndTipV);
			for (unsigned i=0; i<nEndTipV; ++i)
				stream >> m_PartCut[C_wristR][i];
		}
		stream.close();stream.clear();
		//left toe
		path = G_RootFolder+"Template\\toeL.po";	stream.open(path);
		if(!stream)
			MSG_BOX_ERROR("can't find/open file from %s", path.GetBuffer());
		else{
			stream >> buf >> nEndTipV;
			m_PartCut[C_toeL].clear();m_PartCut[C_toeL].resize(nEndTipV);
			for (unsigned i=0; i<nEndTipV; ++i)
				stream >> m_PartCut[C_toeL][i];			
		}
		stream.close();stream.clear();
		//right toe
		path = G_RootFolder+"Template\\toeR.po";	stream.open(path);
		if(!stream)
			MSG_BOX_ERROR("can't find/open file from %s", path.GetBuffer());
		else{
			stream >> buf >> nEndTipV;
			m_PartCut[C_toeR].clear();m_PartCut[C_toeR].resize(nEndTipV);
			for (unsigned i=0; i<nEndTipV; ++i)
				stream >> m_PartCut[C_toeR][i];			
		}
		stream.close();stream.clear();

		if(pSkt==NULL){
			pSkt = this;
			std::cout<<"	^^using the template skeleton to do part cutting.\n"; 
		}
		//find cut link..
		m_CutLink.clear();	m_CutLink.resize(NumBones);
		std::vector<int> RootCuts, EndCuts;
		for (unsigned i=0; i<NumBones; ++i){		
			 
			switch(i){
				case B_neck:
					m_CutLink[i].clear();
					RootCuts.clear();
					RootCuts.push_back(C_headTop);
					EndCuts.clear();					
					EndCuts.push_back(C_neck_sternum);
					EndCuts.push_back(C_neck_clavicleL);
					EndCuts.push_back(C_neck_clavicleR);
					_uti_Optimal_Cut_Link(i,RootCuts,EndCuts,m_CutLink[i],pSkt);
					break;
				case B_sternum:
					RootCuts.clear();			
					RootCuts.push_back(C_neck_sternum);
					RootCuts.push_back(C_sternum_humerusL);
					RootCuts.push_back(C_sternum_humerusR);
					RootCuts.push_back(C_clavicle_sternumR);
					RootCuts.push_back(C_clavicle_sternumL);	
					EndCuts.clear();
					EndCuts.push_back(C_sternum_lumbar);				
					_uti_Optimal_Cut_Link(i,RootCuts,EndCuts,m_CutLink[i],pSkt);
					break;	
				case B_clavicleL:
					RootCuts.clear();
					RootCuts.push_back(C_neck_clavicleL);					
					EndCuts.clear();
					EndCuts.push_back(C_clavicle_humerusL);
					_uti_Optimal_Cut_Link(i,RootCuts,EndCuts,m_CutLink[i],pSkt);
					break;
				case B_clavicleR:
					RootCuts.clear();
					RootCuts.push_back(C_neck_clavicleR);					
					EndCuts.clear();
					EndCuts.push_back(C_clavicle_humerusR);
					_uti_Optimal_Cut_Link(i,RootCuts,EndCuts,m_CutLink[i],pSkt);
					break;
				case B_humerusL:
					RootCuts.clear();
					RootCuts.push_back(C_clavicle_humerusL);					
					EndCuts.clear();
					EndCuts.push_back(C_humerus_RadiusL);
					_uti_Optimal_Cut_Link(i,RootCuts,EndCuts,m_CutLink[i],pSkt);
					break;
				case B_humerusR:
					RootCuts.clear();
					RootCuts.push_back(C_clavicle_humerusR);					
					EndCuts.clear();
					EndCuts.push_back(C_humerus_RadiusR);
					_uti_Optimal_Cut_Link(i,RootCuts,EndCuts,m_CutLink[i],pSkt);
					break;
				case B_radiusL:
					RootCuts.clear();
					RootCuts.push_back(C_humerus_RadiusL);					
					EndCuts.clear();
					EndCuts.push_back(C_wristL);
					_uti_Optimal_Cut_Link(i,RootCuts,EndCuts,m_CutLink[i],pSkt);
					break;
				case B_radiusR:
					RootCuts.clear();
					RootCuts.push_back(C_humerus_RadiusR);					
					EndCuts.clear();
					EndCuts.push_back(C_wristR);
					_uti_Optimal_Cut_Link(i,RootCuts,EndCuts,m_CutLink[i],pSkt);
					break;
				case B_lumbar:
					RootCuts.clear();
					RootCuts.push_back(C_sternum_lumbar);					
					EndCuts.clear();
					EndCuts.push_back(C_lumbar_huckleboneL);
					EndCuts.push_back(C_lumbar_huckleboneR);
					_uti_Optimal_Cut_Link(i,RootCuts,EndCuts,m_CutLink[i],pSkt);
					break;
				case B_huckleboneL:
					RootCuts.clear();
					RootCuts.push_back(C_lumbar_huckleboneL);					
					EndCuts.clear();
					EndCuts.push_back(C_hucklebone_femurL);					
					_uti_Optimal_Cut_Link(i,RootCuts,EndCuts,m_CutLink[i],pSkt);
					break;
				case B_huckleboneR:
					RootCuts.clear();
					RootCuts.push_back(C_lumbar_huckleboneR);					
					EndCuts.clear();
					EndCuts.push_back(C_hucklebone_femurR);					
					_uti_Optimal_Cut_Link(i,RootCuts,EndCuts,m_CutLink[i],pSkt);
					break;
				case B_femurL:
					RootCuts.clear();
					RootCuts.push_back(C_hucklebone_femurL);					
					EndCuts.clear();
					EndCuts.push_back(C_femur_tibiaL);					
					_uti_Optimal_Cut_Link(i,RootCuts,EndCuts,m_CutLink[i],pSkt);
					break;
				case B_femurR:
					RootCuts.clear();
					RootCuts.push_back(C_hucklebone_femurR);					
					EndCuts.clear();
					EndCuts.push_back(C_femur_tibiaR);					
					_uti_Optimal_Cut_Link(i,RootCuts,EndCuts,m_CutLink[i],pSkt);
					break;
				case B_tibiaL:
					RootCuts.clear();
					RootCuts.push_back(C_femur_tibiaL);					
					EndCuts.clear();
					EndCuts.push_back(C_tibia_metatarsalL);					
					_uti_Optimal_Cut_Link(i,RootCuts,EndCuts,m_CutLink[i],pSkt);
					break;
				case B_tibiaR:
					RootCuts.clear();
					RootCuts.push_back(C_femur_tibiaR);					
					EndCuts.clear();
					EndCuts.push_back(C_tibia_metatarsalR);					
					_uti_Optimal_Cut_Link(i,RootCuts,EndCuts,m_CutLink[i],pSkt);
					break;
				case B_metatarsalL:
					RootCuts.clear();
					RootCuts.push_back(C_tibia_metatarsalL);					
					EndCuts.clear();
					EndCuts.push_back(C_toeL);					
					_uti_Optimal_Cut_Link(i,RootCuts,EndCuts,m_CutLink[i],pSkt);
					break;
				case B_metatarsalR:
					RootCuts.clear();
					RootCuts.push_back(C_tibia_metatarsalR);					
					EndCuts.clear();
					EndCuts.push_back(C_toeR);					
					_uti_Optimal_Cut_Link(i,RootCuts,EndCuts,m_CutLink[i],pSkt);
					break;
			}
		}
		return true;
	} 
	
	void _uti_Optimal_Cut_Link(int ptidx, std::vector<int> &RootCuts,std::vector<int> &EndCuts, std::vector< pair<int,int> > &cutlink, CSkeleton* pSkt=NULL)
	{
		if(pSkt == NULL) pSkt = this;
		cutlink.clear();
		unsigned nRootV(0), nEndV(0);
		int TreeOrdrBone = m_LabelOrder2TreeOrder_Bone[ptidx]; 
		
		Vector_3 bone = Vector_3(pSkt->m_BoneVct[TreeOrdrBone].x, pSkt->m_BoneVct[TreeOrdrBone].y, pSkt->m_BoneVct[TreeOrdrBone].z);			
		Vector_3 l ;
		for (unsigned i=0; i<RootCuts.size(); ++i){

			nRootV=  m_PartCut[RootCuts[i]].size();						
			for (unsigned ii=0; ii<nRootV; ++ii){
				
				int thisV = m_PartCut[RootCuts[i]][ii];
				//search in all endV
				FT x, max(0); 		
				int mV;
				for(unsigned j=0; j<EndCuts.size(); ++j){
					nEndV =m_PartCut[ EndCuts[j] ].size();
					for(unsigned k=0; k<nEndV; ++k){
						l = m_pMesh->VP(m_PartCut[ EndCuts[j] ][k]) - m_pMesh->VP(thisV); 	
						x = fabs( l*bone / sqrt(l.squared_length() *bone.squared_length()));
						if(x>max){
							max = x;
							mV = m_PartCut[ EndCuts[j] ][k];
						}
					}
				}	
				cutlink.push_back( make_pair(thisV,mV) );
			}					
		}	
	}

	void gl_draw_PartCut( CPseudoColorRGB &PsdC, bool bUsenormal=true )
	{
		GLubyte PsdColor[3]= {255, 255, 255};
		PsdC.SetPCValueRange(0.0, 1.0);
		FT abs_max;
		abs_max = NumPartCut;

		::glPushMatrix();
		::glPushAttrib(GL_ALL_ATTRIB_BITS);
		::glEnable(GL_LIGHTING);
		::glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
		::glEnable(GL_COLOR_MATERIAL);
		::glEnable(GL_BLEND);
		::glPointSize(2.5);
		::glBegin(GL_POINTS);
		Normal n;Point_3 p; 
		if(bUsenormal){
			for (unsigned i=0; i<NumPartCut; ++i){
				unsigned nRingV = m_PartCut[i].size();	
				PsdC.GetPC(PsdColor, (FT)i/(FT)abs_max);
				::glColor3ubv(PsdColor);
				for(unsigned j=0; j<nRingV; ++j){
				
					n = m_pMesh->VN(m_PartCut[i][j]);	
					glNormal3F(n[0], n[1], n[2]);
					Point_3 p = m_pMesh->VP(m_PartCut[i][j]);			
					glVertex3F(p[0], p[1], p[2]);
				}
			}
		}
		else{
			for (unsigned i=0; i<NumPartCut; ++i){
				unsigned nRingV = m_PartCut[i].size();	
				PsdC.GetPC(PsdColor, (FT)i/(FT)abs_max);
				::glColor3ubv(PsdColor);
				for(unsigned j=0; j<nRingV; ++j){					
					Point_3 p = m_pMesh->VP(m_PartCut[i][j]);			
					glVertex3F(p[0], p[1], p[2]);
				}
			}
		}
		::glEnd();
		//draw cut link
		::glDisable(GL_LIGHTING);
		::glLineWidth(1.75);		
		::glBegin(GL_LINES);		
		for (unsigned i=0; i<NumBones; ++i){
			PsdC.GetPC(PsdColor, (FT)i/(FT)abs_max);
			::glColor3ubv(PsdColor);
			for(unsigned j=0; j<m_CutLink[i].size(); ++j){
				Point_3 p1 = m_pMesh->VP(m_CutLink[i][j].first);			
				Point_3 p2 = m_pMesh->VP(m_CutLink[i][j].second);	
				glVertex3F(p1[0], p1[1], p1[2]);
				glVertex3F(p2[0], p2[1], p2[2]);
			}
		}
		::glEnd();
		::glPopAttrib();
		::glPopMatrix();
	}

};

template <class Polyhedron, class Kernel>
void CSkeleton<Polyhedron, Kernel>::BoneTransform(std::vector<Vector3> &NewJointPos)
{
	if(NewJointPos.size()!=NumJoints){
		std::cout<<"now joint position not enought. \n"; 		return;
	}
	//calc new bone vct
	std::vector<Vector3> m_NewBoneVct(NumBones);
	m_BoneTransform.clear();	m_BoneTransform.resize(NumBones);

	for (int i=0; i<NumBones;++i)
	{
		int prevV = m_Bone2JointIdxs[i].first;
		int otherV = m_Bone2JointIdxs[i].second;

		//rotation and scale
		Vector3 oldBone = m_JointPos[otherV]- m_JointPos[prevV];
		Vector3 newBone = NewJointPos[otherV]- NewJointPos[prevV];
		
		FT angle = newBone.dot(oldBone) / newBone.magnitude() / oldBone.magnitude();
		FT jiao = Acos( CLIP(angle,-1.0 ,1.0) );
		std::cout<<jiao<<" \n"; 
		
		Vector3 ax = oldBone.cross(newBone);
		angle = ML_DEG_TO_RAD * jiao;
		FT scale = newBone.magnitude()/oldBone.magnitude();
		Quaternion<> q  = Quaternion<>(ax , angle);
	
		m_BoneTransform[i] = Transform<>(NewJointPos[prevV]) * Transform<>(q, scale) * Transform<>(-m_JointPos[prevV]);
		assert(otherV==i+1);
	}
}

template <class Polyhedron, class Kernel>
void CSkeleton<Polyhedron, Kernel>::LBSDeformation(std::vector<Vector3> &NewJointPos)
{
	if(m_BoneWeights.size()!=m_nMeshV){
		std::cout<<"bone weights are not enough.\n"; 	return;
	}
	for (unsigned i=0; i<m_nMeshV; ++i){
		Point_3 &p = m_pMesh->VP(i);
		Vector3 v(p.x(),p.y(),p.z());
		Vector3 new_pos(.0,.0,.0);
		std::multimap<int,int>::const_iterator itb = m_JointTree.begin();
		for (unsigned j=0; j<NumBones; ++j,++itb){
			FT w = m_BoneWeights[i][j];
			Vector3 vb = v - m_JointPos[m_Bone2JointIdxs[j].first];
			new_pos += m_BoneTransform[j]*v*w;			
		}
		p = Point_3(new_pos.x, new_pos.y, new_pos.z);
	}
}

template <class Polyhedron, class Kernel>
bool CSkeleton<Polyhedron, Kernel>::pick_joints_by_idx( int jidx )
{
	if(jidx<0 || jidx > NumJoints-1)	return false;
	if(picked_joints.end() != picked_joints.find(jidx))	return false;
	else{
		picked_joints.insert(jidx);	return true;
	}
}

template <class Polyhedron, class Kernel>
bool CSkeleton<Polyhedron, Kernel>::pick_bones_by_idx( int bidx )
{
	if(bidx<0 || bidx > NumJoints-1)	return false;
	if(picked_bones.end() != picked_bones.find(bidx))	return false;
	else{
		picked_bones.insert(bidx);	return true;
	}
}


template<class Polyhedron, class Kernel>
string
CSkeleton<Polyhedron,Kernel>::m_strJointLabel[NumJoints]={
	"chest","navel","waist","head",
	"pelvisR","kneeR","ankleR","footR",
	"pelvisL","kneeL","ankleL","footL",
	"shoulderR","elbowR","handR",
	"shoulderL","elbowL","handL"
};

template<class Polyhedron, class Kernel>
string
CSkeleton<Polyhedron,Kernel>::m_strBoneLabel[NumBones]={
	"B_sternum","B_lumbar","B_neck",
	"B_huckleboneR","B_femurR","B_tibiaR","B_metatarsalR",
	"B_huckleboneL","B_femurL","B_tibiaL","B_metatarsalL",
	"B_clavicleR","B_humerusR","B_radiusR",
	"B_clavicleL","B_humerusL","B_radiusL"
};

template <class Polyhedron, class Kernel>
void CSkeleton<Polyhedron, Kernel>::LoadBoneWeights( std::ifstream &stream )
{
	for (unsigned i=0; i<m_BoneWeights.size(); ++i)		
		SAFE_DELETE_ARRAY(m_BoneWeights[i]);
	m_BoneWeights.clear(); m_BoneWeights.resize(m_nMeshV,NULL);
	for (unsigned i=0; i<m_nMeshV; ++i){
		m_BoneWeights[i] = new FT[NumBones];
		assert(m_BoneWeights[i]); 
	}	
	for	(unsigned i=0; i<m_nMeshV; ++i){
 		stream >> m_BoneWeights[i][0] >> m_BoneWeights[i][1] >> m_BoneWeights[i][2] 
 			   >> m_BoneWeights[i][3] >> m_BoneWeights[i][4] >> m_BoneWeights[i][5]
 			   >> m_BoneWeights[i][6] >> m_BoneWeights[i][7] >> m_BoneWeights[i][8] 
 			   >> m_BoneWeights[i][9] >> m_BoneWeights[i][10] >> m_BoneWeights[i][11]
 			   >> m_BoneWeights[i][12] >> m_BoneWeights[i][13]>> m_BoneWeights[i][14]
 			   >> m_BoneWeights[i][15] >> m_BoneWeights[i][16] ;
	}
	std::cout<<"BoneWeights has been loaded.\n"; 
};

template<class Polyhedron, class Kernel>
int
CSkeleton<Polyhedron,Kernel>::m_LabelOrder2TreeOrder_Bone[NumBones]=
{0,4,1,5,7,8,9,6,10,11,12,2,13,14,3,15,16};

